#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include <queue>

#include "test_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

class FibonacciActionClientGuardCondition : public rclcpp::Node
{
public:
    using Fibonacci = test_interfaces::action::Fibonacci;
    using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

    explicit FibonacciActionClientGuardCondition(const std::string &test, const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
        : Node("fibonacci_action_client", options)
    {
        this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
            this,
            "fibonacci");

        // 构造参数
        for (int i = 0; i < 3; i++)
        {
            Fibonacci::Goal goal_msg = Fibonacci::Goal();
            goal_msg.order = i + 4;
            this->goal_list.push(goal_msg);
        }

        this->guard_condition_ = std::make_shared<rclcpp::GuardCondition>();
        wait_set_.add_guard_condition(this->guard_condition_);
        // wait_set_.add_waitable(this->client_ptr_);

        std::thread([&]()
                    { this->my_loop(); })
            .detach();
    }

    void my_loop()
    {
        this->send_goal();

        auto wait_result = wait_set_.wait(std::chrono::seconds(10));

        if (wait_result.kind() == rclcpp::WaitResultKind::Ready)
        {
            if (wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[0])
            {
                RCLCPP_INFO(this->get_logger(), "guard_condition");
                this->goal_list.pop();
                this->new_goal = false;
            }
            else if (wait_result.get_wait_set().get_rcl_wait_set().clients[0])
            {
                RCLCPP_INFO(this->get_logger(), "rclcpp action client");
            }
            else
            {
                RCLCPP_INFO(this->get_logger(), "unknown waitset type");
            }
        }
        else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout)
        {
            RCLCPP_ERROR(this->get_logger(), "Wait-set failed with timeout");
        }
    }

    void send_goal()
    {
        using namespace std::placeholders;

        if (!this->client_ptr_->wait_for_action_server())
        {
            RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
            rclcpp::shutdown();
        }

        this->new_goal = true;

        auto goal_msg = this->goal_list.front();
        auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();

        // send_goal_options.goal_response_callback = std::bind(&FibonacciActionClientGuardCondition::goal_response_callback, this, _1);
        // send_goal_options.feedback_callback = std::bind(&FibonacciActionClientGuardCondition::feedback_callback, this, _1, _2);

        send_goal_options.result_callback = std::bind(&FibonacciActionClientGuardCondition::result_callback, this, _1);
        this->client_ptr_->async_send_goal(goal_msg, send_goal_options);

        RCLCPP_INFO(this->get_logger(), "Sending goal");
    }

private:
    rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
    std::queue<Fibonacci::Goal> goal_list;
    bool new_goal = false;
    rclcpp::WaitSet wait_set_;
    rclcpp::GuardCondition::SharedPtr guard_condition_;

    void goal_response_callback(const GoalHandleFibonacci::SharedPtr &goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
            this->guard_condition_->trigger();
            this->new_goal = false;
        }
        else
        {
            std::string uuid = rclcpp_action::to_string(goal_handle->get_goal_id());
            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, id is %s", uuid.c_str());
        }
    }

    void feedback_callback(
        GoalHandleFibonacci::SharedPtr,
        const std::shared_ptr<const Fibonacci::Feedback> feedback)
    {
        std::stringstream ss;
        ss << "Next number in sequence received: ";
        for (auto number : feedback->partial_sequence)
        {
            ss << number << " ";
        }
        RCLCPP_INFO(this->get_logger(), ss.str().c_str());
    }

    void result_callback(const GoalHandleFibonacci::WrappedResult &result)
    {
        std::string uuid = rclcpp_action::to_string(result.goal_id);
        RCLCPP_INFO(this->get_logger(), "Goal id is %s, finished", uuid.c_str());

        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
            return;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
            return;
        default:
            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
            return;
        }

        std::stringstream ss;
        ss << "Result received: ";
        for (auto number : result.result->sequence)
        {
            ss << number << " ";
        }
        RCLCPP_INFO(this->get_logger(), ss.str().c_str());

        this->guard_condition_->trigger();
    }
};